{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Frenet坐标系与Cartesian坐标系转换\n",
    "\n",
    "博客地址：https://blog.csdn.net/weixin_42301220/article/details/125211683?spm=1001.2014.3001.5501\n",
    "\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### 1. Cartesian转 Frenet公式\n",
    "\n",
    "<center><img src=\"https://img-blog.csdnimg.cn/40beefb1d46f4a2ba8a1264c6a73c9c4.png\" width=50%>"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "from math import *\n",
    "\n",
    "def cartesian2Frenet(x,y,theta_x,v_x,a_x,k_x,s_r,x_r,y_r,theta_r,k_r,k_r_):\n",
    "    \"\"\"全局坐标系转Frenet坐标系\n",
    "\n",
    "    Args:\n",
    "        x (_type_): Cartesian坐标系下的车辆横坐标位置\n",
    "        y (_type_): Cartesian坐标系下的车辆纵坐标位置\n",
    "        theta_x (_type_): 为方位角，即全局坐标系下的朝向；\n",
    "        v_x (_type_): Cartesian坐标系下的线速度大小;\n",
    "        a_x (_type_): Cartesian坐标系下的加速度\n",
    "        k_x (_type_): 曲率\n",
    "        s_r (_type_): 投影点的弧长\n",
    "        x_r (_type_): 投影点P点在Cartesian坐标系下的x坐标\n",
    "        y_r (_type_): 投影点P点在Cartesian坐标系下的y坐标\n",
    "        theta_r (_type_): 投影点P点在Cartesian坐标系下的朝向角\n",
    "        k_r (_type_): 曲率\n",
    "        k_r_ (_type_): 曲率对弧长s的一阶导数_\n",
    "\n",
    "    Returns:\n",
    "        _type_: Frenet坐标系下车辆的运动状态\n",
    "    \"\"\"\n",
    "    delta_theta = theta_x-theta_r\n",
    "    one_kr_d = 1-k_r*d\n",
    "    s=s_r\n",
    "    d=np.sign((y-y_r)*cos(theta_r)-(x-x_r)*sin(theta_r))*sqrt((x-x_r)**2+(y-y_r)**2)\n",
    "    dot_d = v_x*sin(delta_theta)\n",
    "    ddot_d = a_x*sin(delta_theta)\n",
    "    dot_s=v_x*cos(delta_theta)/one_kr_d\n",
    "    d_=one_kr_d*tan(delta_theta)\n",
    "    d__=-(k_r_*d+k_r*d_)*tan(delta_theta)+one_kr_d/(cos(delta_theta))**2*(k_x*one_kr_d/cos(delta_theta)-k_r)\n",
    "    ddot_s = (a_x*cos(delta_theta)-dot_s**2*(d_*(k_x*one_kr_d/cos(delta_theta)-k_r)-(k_r_*d+k_r*d_)))/one_kr_d\n",
    "\n",
    "    return s,dot_s,ddot_s,d,dot_d,ddot_d,d_,d__\n",
    "    "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### 2.Frenet转Cartesian公式\n",
    "\n",
    "<center><img src=\"https://img-blog.csdnimg.cn/929c2c9d2e3e4f73b6c5d9f78fac3bd9.png\" width=50%>"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "interpreter": {
   "hash": "0c7484b3574347463e16b31029466871583b0d4e5c4ad861e8848f2d3746b4de"
  },
  "kernelspec": {
   "display_name": "Python 3.8.12 ('gobigger')",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.8.12"
  },
  "orig_nbformat": 4
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
